#include "LidarIris.h"
#include "fftm/fftm.hpp"
#include<opencv2/core/core_c.h>

const int NUM = 5;
const double MIDI = 0.3;
/**
 * @brief 获得雷达虹膜全局描述符
 *
 * @param[in] cloud   输入点云
 * @return IrisMap    输出描述符
 */
cv::Mat1b LidarIris::GetIris(const pcl::PointCloud<pcl::PointXYZI> &cloud)
{
    cv::Mat1b IrisMap = cv::Mat1b::zeros(80, 360);

    // 16-line
    // for (pcl::PointXYZ p : cloud.points)
    // {
    //     float dis = sqrt(p.data[0] * p.data[0] + p.data[1] * p.data[1]);
    //     float arc = (atan2(p.data[2], dis) * 180.0f / M_PI) + 15;
    //     float yaw = (atan2(p.data[1], p.data[0]) * 180.0f / M_PI) + 180;
    //     int Q_dis = std::min(std::max((int)floor(dis), 0), 79);
    //     int Q_arc = std::min(std::max((int)floor(arc / 4.0f), 0), 7);
    //     int Q_yaw = std::min(std::max((int)floor(yaw + 0.5), 0), 359);
    //     IrisMap.at<uint8_t>(Q_dis, Q_yaw) |= (1 << Q_arc);
    // }

    // 64-line
    for (pcl::PointXYZI p : cloud.points)
    {
        float dis = sqrt(p.data[0] * p.data[0] + p.data[1] * p.data[1]);
        //? +24是干啥的
        //| 可能64线垂直视角在-24～40内
        float arc = (atan2(p.data[2], dis) * 180.0f / M_PI) + 24;
        //| +180是确保水平视角在0～360内
        float yaw = (atan2(p.data[1], p.data[0]) * 180.0f / M_PI) + 180;

        int Q_dis = std::min(std::max((int)floor(dis), 0), 79);
        int Q_arc = std::min(std::max((int)floor(arc / 4.0f), 0), 7);
        // int Q_arc = floor(arc*0.28);
        int Q_yaw = std::min(std::max((int)floor(yaw + 0.5), 0), 359);
        //!|= 按位或
        //| 每一个at中存放一个8位的二级制数用于表示一个栅格
        //| 1<<n 就是1先转成二进制  在左移n位  然后补0
        //| 比如 1<<2 1的二进制为 0000 0001 左移2位 0000 0100.
        IrisMap.at<uint8_t>(Q_dis, Q_yaw) |= (1 << Q_arc);
    }
    return IrisMap;
}

cv::Mat1b LidarIris::GetIrisWithI(const pcl::PointCloud<pcl::PointXYZI> &cloud)
{
    cv::Mat1b IrisMap = cv::Mat1b::zeros(80, 360);
    std::vector<std::vector<float>> SaveI(80, std::vector<float>(360, 0.0));
    std::vector<std::vector<float>> CountI(80, std::vector<float>(360, 0.0));
    for (pcl::PointXYZI p : cloud.points)
    {
        float I = p.intensity;
        int i = 0;
        i++;
        float dis = sqrt(p.data[0] * p.data[0] + p.data[1] * p.data[1]);
        //? +24是干啥的
        //| 可能64线垂直视角在-24～40内
        float arc = (atan2(p.data[2], dis) * 180.0f / M_PI) + 24;
        //| +180是确保水平视角在0～360内
        float yaw = (atan2(p.data[1], p.data[0]) * 180.0f / M_PI) + 180;
        int Q_dis = std::min(std::max((int)floor(dis), 0), 79);
        int Q_arc = floor(arc * 0.24); // 分7层
        int Q_yaw = std::min(std::max((int)floor(yaw + 0.5), 0), 359);
        //!|= 按位或
        //|每一个at中存放一个8位的二级制数用于表示一个栅格
        //| 1<<n 就是1先转成二进制  在左移n位  然后补0
        //| 比如 1<<2 1的二进制为 0000 0001 左移2位 0000 0100.
        if (Q_arc != NUM)
        IrisMap.at<uint8_t>(Q_dis, Q_yaw) |= (1 << Q_arc);
        SaveI[Q_dis][Q_yaw] += I;
        CountI[Q_dis][Q_yaw]++;

    }
    for (int i = 0; i < IrisMap.rows; i++)
    {
        for (int j = 0; j < IrisMap.cols; j++)
        {
            float smidI;

            smidI = SaveI[i][j] / CountI[i][j];
            
            if (smidI >= MIDI)
                IrisMap.at<uint8_t>(i, j) |= (1 << NUM);
        }
    }
    return IrisMap;
}

void LidarIris::UpdateFrame(const cv::Mat1b &frame, int frameIndex, float *matchDistance, int *matchIndex)
{
    // first: calc feature
    std::vector<float> vec;
    auto feature = GetFeature(frame, vec);
    flann::Matrix<float> queries(vec.data(), 1, vec.size());
    if (featureList.size() == 0)
    {
        if (matchDistance)
            *matchDistance = NAN;
        if (matchIndex)
            *matchIndex = -1;
        vecList.buildIndex(queries);
    }
    else
    {
        // second: search in database
        vecList.knnSearch(queries, indices, dists, _matchNum, flann::SearchParams(32));
        // thrid: calc matches
        std::vector<float> dis(_matchNum);
        for (int i = 0; i < _matchNum; i++)
        {
            dis[i] = Compare(feature, featureList[indices[0][i]]);
        }
        int minIndex = std::min_element(dis.begin(), dis.end()) - dis.begin();
        if (matchDistance)
            *matchDistance = dis[minIndex];
        if (matchIndex)
            *matchIndex = frameIndexList[indices[0][minIndex]];
        // forth: add frame to database
        vecList.addPoints(queries);
    }
    featureList.push_back(feature);
    frameIndexList.push_back(frameIndex);
}

float LidarIris::Compare(const LidarIris::FeatureDesc &img1, const LidarIris::FeatureDesc &img2, int *bias)
{
    auto firstRect = FFTMatch(img2.img, img1.img); // 没有特征提取
    //|rr.center = tr + Point2d(im0.cols / 2, im0.rows / 2);
    int firstShift = firstRect.center.x - img1.img.cols / 2;
    float dis1;
    int bias1;
    GetHammingDistance(img1.T, img1.M, img2.T, img2.M, firstShift, dis1, bias1);
    //
    auto T2x = circShift(img2.T, 0, 180); // 移动180度
    auto M2x = circShift(img2.M, 0, 180);
    auto img2x = circShift(img2.img, 0, 180);
    //
    auto secondRect = FFTMatch(img2x, img1.img);
    int secondShift = secondRect.center.x - img1.img.cols / 2;
    float dis2 = 0;
    int bias2 = 0;
    GetHammingDistance(img1.T, img1.M, T2x, M2x, secondShift, dis2, bias2);
    //
    if (dis1 < dis2)
    {
        if (bias)
            *bias = bias1;
        return dis1;
    }
    else
    {
        if (bias)
            *bias = (bias2 + 180) % 360;
        return dis2;
    }
}

/**
 * @brief
 *
 * @param[in] src
 * @param[in] nscale  4
 * @param[in] minWaveLength  18
 * @param[in] mult 1.6
 * @param[in] sigmaOnf 0.75
 * @return std::vector<cv::Mat2f>
 */
std::vector<cv::Mat2f> LidarIris::LogGaborFilter(const cv::Mat1f &src, unsigned int nscale, int minWaveLength, double mult, double sigmaOnf)
{
    //|获得描述符的行数和列数
    int rows = src.rows;
    int cols = src.cols;

    //             |[x0  [x2     [xn-1 |
    //|filtersum = |                   |
    //             | x1], x3],... xn]  |
    cv::Mat2f filtersum = cv::Mat2f::zeros(1, cols);

    std::vector<cv::Mat2f> EO(nscale);

    //|确保ndata为偶数
    int ndata = cols;
    if (ndata % 2 == 1)
        ndata--;

    cv::Mat1f logGabor = cv::Mat1f::zeros(1, ndata);
    cv::Mat2f result = cv::Mat2f::zeros(rows, ndata);

    cv::Mat1f radius = cv::Mat1f::zeros(1, ndata / 2 + 1);
    radius.at<float>(0, 0) = 1;
    for (int i = 1; i < ndata / 2 + 1; i++)
    {
        radius.at<float>(0, i) = i / (float)ndata;
    }

    double wavelength = minWaveLength; // 波长 周期
    for (int s = 0; s < nscale; s++)
    {
        double fo = 1.0 / wavelength; // 频率
        double rfo = fo / 0.5;        // 2倍的频率
        //
        cv::Mat1f temp; //(radius.size());
        cv::log(radius / fo, temp);
        cv::pow(temp, 2, temp);
        cv::exp((-temp) / (2 * log(sigmaOnf) * log(sigmaOnf)), temp);
        temp.copyTo(logGabor.colRange(0, ndata / 2 + 1));
        //
        logGabor.at<float>(0, 0) = 0;
        cv::Mat2f filter;
        cv::Mat1f filterArr[2] = {logGabor, cv::Mat1f::zeros(logGabor.size())};
        cv::merge(filterArr, 2, filter);
        filtersum = filtersum + filter;
        for (int r = 0; r < rows; r++)
        {
            cv::Mat2f src2f;
            cv::Mat1f srcArr[2] = {src.row(r).clone(), cv::Mat1f::zeros(1, src.cols)};
            cv::merge(srcArr, 2, src2f);
            cv::dft(src2f, src2f);
            //! mulSpectrums对两个由实数傅里叶变换或复数傅里叶变换得到的矩阵或复数矩阵进行每个元素的乘法运算。
            // src2f = src2f*filter
            cv::mulSpectrums(src2f, filter, src2f, 0);
            //|傅里叶逆变换
            cv::idft(src2f, src2f);
            src2f.copyTo(result.row(r));
        }
        EO[s] = result.clone();
        wavelength *= mult;
    }
    filtersum = circShift(filtersum, 0, cols / 2);
    return EO;
}

/**
 * @brief
 *
 * @param[in] src
 * @param[in] nscale
 * @param[in] minWaveLength
 * @param[in] mult
 * @param[in] sigmaOnf
 * @param[in] T
 * @param[in] M
 */

void LidarIris::LoGFeatureEncode(const cv::Mat1b &src, unsigned int nscale, int minWaveLength, double mult, double sigmaOnf, cv::Mat1b &T, cv::Mat1b &M)
{
    cv::Mat1f srcFloat;
    src.convertTo(srcFloat, CV_32FC1);
    // nscale = 4
    // minWaveLength = 18
    // mult = 1.6
    // sigmaOnf =0.75
    auto list = LogGaborFilter(srcFloat, nscale, minWaveLength, mult, sigmaOnf);
    //std::cout << list.size() << std::endl;
    std::vector<cv::Mat1b> Tlist(nscale * 2), Mlist(nscale * 2);
    //| 计算M和T
    for (int i = 0; i < list.size(); i++)
    {
        cv::Mat1f arr[2];
        cv::split(list[i], arr);
        //|对实部进行二值化
        Tlist[i] = arr[0] > 0;          //! 像素大于0的取255,小于等于0的取0
        Tlist[i + nscale] = arr[1] > 0; // 二值化1
        cv::Mat1f m;
        cv::magnitude(arr[0], arr[1], m);
        //|对幅值进行二值化
        Mlist[i] = m < 0.0001;          //! 像素为0的取255,大于0的取0
        Mlist[i + nscale] = m < 0.0001; // 二值化2
    }
    cv::vconcat(Tlist, T);
    cv::vconcat(Mlist, M);
    // cv::imshow("T",T);
    // cv::imshow("m",M);
}

/**
 * @brief
 *
 * @param[in] src
 * @return LidarIris::FeatureDesc
 */
LidarIris::FeatureDesc LidarIris::GetFeature(const cv::Mat1b &src)
{
    FeatureDesc desc;
    desc.img = src;
    //! 使用LoG-Gabor 滤波器 提取特征点
    // _nscale = 4
    // _minWaveLength = 18
    // _mult = 1.6
    // _sigmaOnf =0.75
    LoGFeatureEncode(src, _nscale, _minWaveLength, _mult, _sigmaOnf, desc.T, desc.M);
    return desc;
}

/**
 * @brief
 *
 * @param[in] src
 * @param[in] vec
 * @return LidarIris::FeatureDesc
 */
LidarIris::FeatureDesc LidarIris::GetFeature(const cv::Mat1b &src, std::vector<float> &vec)
{
    cv::Mat1f temp;
    src.convertTo(temp, CV_32FC1);
    cv::reduce((temp != 0) / 255, temp, 1, CV_REDUCE_AVG);
    vec = temp.isContinuous() ? temp : temp.clone();
    return GetFeature(src);
}

/**
 * @brief  用于计算二值化后的两幅图像的偏差
 *
 * @param[in] T1       图像经过Gabor特征提取后对其实部进行二值化
 * @param[in] M1       图像经过Gabor特征提取后对其幅值进行二值化
 * @param[in] T2
 * @param[in] M2
 * @param[in] scale
 * @param[in] dis
 * @param[in] bias
 */
void LidarIris::GetHammingDistance(const cv::Mat1b &T1, const cv::Mat1b &M1, const cv::Mat1b &T2, const cv::Mat1b &M2, int scale, float &dis, int &bias)
{
    dis = NAN;
    bias = -1;
    for (int shift = scale - 2; shift <= scale + 2; shift++) /// scale为x方向上平移的估计值,扩大平移范围为
    {
        //| 统一到相同位姿下
        cv::Mat1b T1s = circShift(T1, 0, shift); // 二值图一
        cv::Mat1b M1s = circShift(M1, 0, shift); // 二值图二
        //| M图的存储方式是图像的幅值为空取1
        //| 两幅图中的值同时为空时取1
        cv::Mat1b mask = M1s | M2;
        //| 值为空的个数
        int MaskBitsNum = cv::sum(mask / 255)[0]; // 像素为255的个数
        //| 值不为空的个数
        int totalBits = T1s.rows * T1s.cols - MaskBitsNum;
        //| 相似度(图像黑色区域越多,越相似)
        cv::Mat1b C = T1s ^ T2; // 异或:相同为0不同为1
        //| 令不相同的区域且存在不为空的区域为1(白),其余地方为0(黑)
        //| 提高相似度的可靠性
        C = C & ~mask;
        //| 简化差异性,记录不同区域的个数
        int bitsDiff = cv::sum(C / 255)[0];
        //| 如果totalBits == 0 则说明两幅图的值全为空,无解
        if (totalBits == 0)
        {
            dis = NAN;
        }
        else
        {
            //| 进一步对差异性进行处理
            //| M中值不为空的个数越多,越相似
            //| T中不相同的区域越少,越相似
            float currentDis = bitsDiff / (float)totalBits;
            //| 如果获得更好的相似度或者相似度的值为空则更新
            if (currentDis < dis || isnan(dis)) //! isnan用于判断dis值是否为空
            {
                dis = currentDis;
                //| 获得最佳相似的对于的偏移量
                bias = shift;
            }
        }
    }
    return;
}

inline cv::Mat LidarIris::circRowShift(const cv::Mat &src, int shift_m_rows)
{
    if (shift_m_rows == 0)
        return src.clone();
    shift_m_rows %= src.rows;
    int m = shift_m_rows > 0 ? shift_m_rows : src.rows + shift_m_rows;
    cv::Mat dst(src.size(), src.type());
    src(cv::Range(src.rows - m, src.rows), cv::Range::all()).copyTo(dst(cv::Range(0, m), cv::Range::all()));
    src(cv::Range(0, src.rows - m), cv::Range::all()).copyTo(dst(cv::Range(m, src.rows), cv::Range::all()));
    return dst;
}

/**
 * @brief
 *
 * @param[in] src
 * @param[in] shift_n_cols
 * @return cv::Mat
 */
inline cv::Mat LidarIris::circColShift(const cv::Mat &src, int shift_n_cols)
{
    if (shift_n_cols == 0)
        return src.clone();
    //! 如果有i = a/2%a,则i = a/2
    //! 这里输入的shift_n_cols = cols/2
    shift_n_cols %= src.cols; //! 这样计算后 shift_n_cols still= cols/2
    int n = shift_n_cols > 0 ? shift_n_cols : src.cols + shift_n_cols;
    cv::Mat dst(src.size(), src.type());
    //! 提取第1到3列（不包括3）
    //! Mat B = A(Range::all(),Range(1,3));
    //! 提取B的第5至9行（不包括9）
    //! C= B(Range(5,9),Range::all());
    src(cv::Range::all(), cv::Range(src.cols - n, src.cols)).copyTo(dst(cv::Range::all(), cv::Range(0, n)));
    src(cv::Range::all(), cv::Range(0, src.cols - n)).copyTo(dst(cv::Range::all(), cv::Range(n, src.cols)));
    return dst;
}

cv::Mat LidarIris::circShift(const cv::Mat &src, int shift_m_rows, int shift_n_cols)

{
    return circColShift(circRowShift(src, shift_m_rows), shift_n_cols);
}

void LidarIris::makeAndSaveLidarIris(pcl::PointCloud<pcl::PointXYZI>::Ptr &_iris_down)
{
    // Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1
    cv::Mat1b li = GetIrisWithI(*_iris_down);
    // polarcontexts_.push_back(li);
}

std::pair<float, int> LidarIris::distanceBtnLidarIris( cv::Mat1b &_sc1,cv::Mat1b &_sc2 )
{
     FeatureDesc fd1 = GetFeature(_sc1);
     FeatureDesc fd2 = GetFeature(_sc2);

     int bias;
     auto dis = Compare(fd1, fd2, &bias);

     return std::make_pair(dis,bias);
}

cv::Mat1b LidarIris::GetRingKeyFromIris(cv::Mat1b li)
{
    // std::cout<<"li"<<std::endl;
    cv::Mat1b RingKey = cv::Mat1b::zeros(li.rows, 1);
    for (int r = 0; r < li.rows; r++)
    {
        int sum = 0;
        for (int c = 0; c < li.cols; c++)
        {
            RingKey.at<uint8_t>(r, 0) = +li.at<uint8_t>(r, c);
        }
    }
    return RingKey;
}
